#include <cassert>

#include "Fixed.h"

#ifdef __GNUC__
#define CLZ64(x) __builtin_clzll(x)
#elif defined(_MSC_VER)
#include <cmnintrin.h>
#define CLZ64(x) _CountLeadingZeros64(x)
#endif

namespace uav
{
namespace math
{

const Fixed Fixed::Inf       = Fixed(internal(), 0x7fffffffffffffffLL);
const Fixed Fixed::Ninf      = Fixed(internal(), 0x8000000000000000LL);
const Fixed Fixed::Two       = Fixed(internal(), 0x0000000020000000LL);
const Fixed Fixed::One       = Fixed(internal(), 0x0000000010000000LL);
const Fixed Fixed::Half      = Fixed(internal(), 0x0000000008000000LL);
const Fixed Fixed::Quarter   = Fixed(internal(), 0x0000000004000000LL);
const Fixed Fixed::Zero      = Fixed(internal(), 0);
const Fixed Fixed::Pi        = Fixed(internal(), 0x000000003243f6a8LL);
const Fixed Fixed::TwoPi     = Fixed(internal(), 0x000000006487ed51LL);
const Fixed Fixed::HalfPi    = Fixed(internal(), 0x000000001921fb54LL);
const Fixed Fixed::QuarterPi = Fixed(internal(), 0x000000000c90fdaaLL);

Fixed::Fixed(double x)
{
	uint64_t bits = *reinterpret_cast<uint64_t*>(&x);
	bool neg = (((bits >> 63) & 0x1) == 1);
	uint16_t exp_bits = (bits >> 52) & 0x7ff;
	int64_t sig_bits = bits & 0xfffffffffffffLL;

	if (exp_bits == 0x000) {
		*this =  Zero;
		return;
	}
	if (exp_bits == 0x7ff) {
		*this = (neg ? Ninf : Inf);
		return;
	}

	int shift = (int(exp_bits) - 1023) - (52 - FIXED_FRAC_BITS);

	if (shift > 11) {
		*this = (neg ? Ninf : Inf);
		return;
	}
	if (shift < -52) {
		*this = Zero;
		return;
	}

	val =  sig_bits | (int64_t(1) << 52);

	if (shift > 0) {
		val <<= shift;
	} else if(shift < 0) {
		val >>= -shift;
	}

	if (neg) {
		val = -val;
	}
}

Fixed::Fixed(float x)
{
	uint32_t bits = *reinterpret_cast<uint32_t*>(&x);
	bool neg = (((bits >> 31) & 0x1) == 1);
	uint8_t exp_bits = (bits >> 23) & 0xff;
	uint32_t sig_bits = bits & 0x7fffff;

	if (exp_bits == 0x00) {
		*this = Zero;
		return;
	}
	if (exp_bits == 0xff) {
		*this = (neg ? Ninf : Inf);
		return;
	}

	int shift = (int(exp_bits) - 127) + (FIXED_FRAC_BITS - 23);

	if (shift > 40) {
		*this = (neg ? Ninf : Inf);
		return;
	}
	if (shift < -23) {
		*this = Zero;
		return;
	}

	val = int64_t(sig_bits) | (int64_t(1) << 23);

	if (shift > 0) {
		val <<= shift;
	} else if(shift < 0) {
		val >>= -shift;
	}

	if (neg) {
		val = -val;
	}
}

double Fixed::toDouble() const
{
	if (val == 0) return 0.0;

	uint64_t bits;
	uint64_t v;

	// determine sign
	if (val < 0) {
		bits = int64_t(1) << 63; // negative, set sign bit
		v = -val;
	} else {
		bits = 0;
		v = val;
	}

	// determine exponent from number of leading zeros
	int lz = CLZ64(v);
	int exp = FIXED_INT_BITS - 1 - lz;
	bits |= uint64_t((exp + 1023) & 0x7ff) << 52; // set exponent bits

	// make normalized significand (leading 1 is implied)
	if (lz > 11) {
		bits |= (v << (lz - 11)) & 0xfffffffffffffLL;
	} else if (lz == 11) {
		bits |= v & 0xfffffffffffffLL;
	} else {
		bits |= (v >> (11 - lz)) & 0xfffffffffffffLL;
	}

	return *reinterpret_cast<double*>(&bits);
}

float Fixed::toFloat() const
{
	if (val == 0) return 0.0f;

	uint32_t bits;
	uint64_t v;

	// determine sign
	if (val < 0) {
		bits = 1 << 31; // negative, set sign bit
		v = -val;
	} else {
		bits = 0;
		v = val;
	}

	// determine exponent from number of leading zeros
	int lz = CLZ64(v);
	int exp = FIXED_INT_BITS - 1 - lz;
	bits |= ((exp + 127) & 0xff) << 23; // set exponent bits

	// make normalized significand (leading 1 is implied)
	// 40 = 32 (single precision bits) + 9 (exp and sign) - 1 (implied bit)
	if (lz > 40) {
		bits |= (v << (lz - 40)) & 0x7fffff;
	} else if (lz == 40) {
		bits |= v  & 0x7fffff;
	} else {
		bits |= (v >> (40 - lz)) & 0x7fffff;
	}

	return *reinterpret_cast<float*>(&bits);
}

// Square root result will have up to 46 (36/2 + 28) non-zero bits
namespace { const int SQRT_RESULT_BITS = (FIXED_INT_BITS >> 1) + FIXED_FRAC_BITS; }

Fixed Fixed::sqrt() const
{
	assert(val >= 0);

	if (val <= 0) {
		return Zero;
	}

	uint64_t lo = val;
	uint64_t hi = 0;
	Fixed root;

	// Count leading zero bits so they can be skipped
	int leadingZeros = CLZ64(lo);
	leadingZeros &= 0xfe; // Need even number
	lo <<= leadingZeros;

	for (int i = (leadingZeros >> 1); i < SQRT_RESULT_BITS; i++) {
		hi = (hi << 2) | (lo >> 62);
		lo <<= 2;
		root.val <<= 1;
		uint64_t test = (root.val << 1) | 1;
		if (hi >= test) {
			hi -= test;
			root.val++;
		}
	}

	return root;
}

// Exponential functions implemented using shift and add algorithms

namespace
{
	// Look-up tables for the exponential and logarithm functions.
	const int64_t LOG_2_POWER_N_REVERSED[] = {
		0x18429946eLL, 0x1791272efLL, 0x16dfb516fLL, 0x162e42ff0LL, 0x157cd0e70LL,
		0x14cb5ecf1LL, 0x1419ecb71LL, 0x13687a9f2LL, 0x12b708872LL, 0x1205966f3LL,
		0x115424573LL, 0x10a2b23f4LL,  0xff140274LL,  0xf3fce0f5LL,  0xe8e5bf75LL,
		 0xddce9df6,  0xd2b77c76,  0xc7a05af7,  0xbc893977,  0xb17217f8,
		 0xa65af679,  0x9b43d4f9,  0x902cb379,  0x851591fa,  0x79fe707b,
		 0x6ee74efb,  0x63d02d7b,  0x58b90bfc,  0x4da1ea7c,  0x428ac8fd,
		 0x3773a77d,  0x2c5c85fe,  0x2145647e,  0x162e42ff,   0xb17217f
	};
	const int64_t LOG_1_PLUS_2_POWER_MINUS_N[] = {
		0x67cc8fb, 0x391fef9, 0x1e27077, 0xf85186, 0x7e0a6c, 0x3f8151,
		 0x1fe02a,   0xff805,   0x7fe01,  0x3ff80,  0x1ffe0,   0xfff8,
		   0x7ffe,    0x4000,    0x2000,   0x1000,    0x800,    0x400,
			0x200,     0x100,      0x80,     0x40,     0x20,     0x10,
			  0x8,       0x4,       0x2,      0x1
	};
	const int64_t LOG_1_OVER_1_MINUS_2_POWER_MINUS_N[] = {
		0xb172180, 0x49a5884, 0x222f1d0, 0x108598b, 0x820aec, 0x408159,
		 0x20202b,  0x100805,   0x80201,   0x40080,  0x20020,  0x10008,
		   0x8002,    0x4001,    0x2000,    0x1000,    0x800,    0x400,
			0x200,     0x100,      0x80,      0x40,     0x20,     0x10,
			  0x8,       0x4,       0x2,       0x1
	};

	const int MAX_POWER = 63 - FIXED_FRAC_BITS;
}

Fixed Fixed::exp() const
{
	if (val >= LOG_2_POWER_N_REVERSED[0]) {
		return Inf;
	}
	if (val < -LOG_2_POWER_N_REVERSED[63 - (FIXED_FRAC_BITS << 1)]) {
		return Zero;
	}
	if (val == 0) {
		return One;
	}

	Fixed res = One;

	if (val > 0) {
		int power = MAX_POWER;
		const int64_t *lut = LOG_2_POWER_N_REVERSED;
		int64_t temp = val;
		while (temp != 0 && power > -FIXED_FRAC_BITS) {
			while (power == 0 || (temp < *lut)) {
				if (power == 0) {
					lut = LOG_1_PLUS_2_POWER_MINUS_N;
				} else {
					lut++;
				}
				power--;
			}
			temp -= *lut;
			if (power < 0) {
				res.val += (res.val >> -power);
			} else {
				res.val <<= power;
			}
		}
	} else {
		int power = FIXED_FRAC_BITS;
		const int64_t *lut = LOG_2_POWER_N_REVERSED + (MAX_POWER - power);
		int64_t temp = val;
		while (temp != 0 && power > -FIXED_FRAC_BITS) {
			while (power == 0 || (temp > -(*lut))) {
				if (power == 0) {
					lut = LOG_1_OVER_1_MINUS_2_POWER_MINUS_N;
				} else {
					lut++;
				}
				power--;
			}
			temp += *lut;
			if (power < 0) {
				res.val -= (res.val >> -power);
			} else {
				res.val >>= power;
			}
		}
	}

	return res;
}

Fixed Fixed::log() const
{
	assert(val > 0);

	if (val <= 0) {
		return Ninf;
	}
	if (val == FIXED_RESOLUTION) {
		return Zero;
	}

	uint64_t temp = val;
	int ls = 0;
	uint64_t scalePosition = 0x8000000000000000LL;
	while (temp < scalePosition) {
		ls++;
		temp <<= 1;
	}

	Fixed res = Fixed(internal(), (ls < MAX_POWER) ?
		LOG_2_POWER_N_REVERSED[ls] : -LOG_2_POWER_N_REVERSED[(MAX_POWER << 1) - ls]);
	int rs = 1;
	uint64_t shiftedTemp = temp >> 1;
	while (temp && (rs < FIXED_FRAC_BITS)) {
		while ((rs < FIXED_FRAC_BITS) && (temp < (shiftedTemp + scalePosition))) {
			shiftedTemp >>= 1;
			rs++;
		}
		temp -= shiftedTemp;
		shiftedTemp = temp >> rs;
		res.val += LOG_1_OVER_1_MINUS_2_POWER_MINUS_N[rs - 1];
	}

	return res;
}

// Trig. functions implemented using CORDIC algorithm in circular mode.

namespace
{
	// Look-up table for the CORDIC algorithms.
	// CORDIC_CIRCULAR_TABLE[i] = arctan(2^(-i))*2^28
	const int32_t CORDIC_CIRCULAR_TABLE[] = {
		210828714, 124459457, 65760959, 33381290, 16755422, 8385879, 4193963,
		2097109, 1048571, 524287, 262144, 131072, 65536, 32768, 16384, 8192,
		4096, 2048, 1024, 512, 256, 128, 64, 32, 16, 8, 4, 2, 1, 0, 0, 0
	};
	const int32_t CORDIC_K = 442048841; // (K = 1.646760258121) * 2^28
	const int32_t CORDIC_INV_K = 163008219; // (1 / K) * 2^28

	int32_t rightShift(int32_t x, int shift)
	{
		return (shift < 0) ? (x << -shift) : (x >> shift);
	}

	void cordicCircularRotation(int32_t &x, int32_t &y, int32_t z)
	{
		const int32_t *lut = CORDIC_CIRCULAR_TABLE;

		for (int i = 0; i <= FIXED_FRAC_BITS; i++) {
			int32_t xShift = rightShift(x, i);
			int32_t yShift = rightShift(y, i);

			if (z < 0) {
				x += yShift;
				y -= xShift;
				z += *(lut++);
			} else {
				x -= yShift;
				y += xShift;
				z -= *(lut++);
			}
		}
	}

	void cordicCircularVectoring(int32_t &x, int32_t y, int32_t &z)
	{
		const int32_t *lut = CORDIC_CIRCULAR_TABLE;

		for (int i = 0; i <= FIXED_FRAC_BITS; i++) {
			int32_t xShift = rightShift(x, i);
			int32_t yShift = rightShift(y, i);

			if ((y < 0 && x > 0) || (y > 0 && x < 0)) {
				x -= yShift;
				y += xShift;
				z -= *(lut++);
			} else {
				x += yShift;
				y -= xShift;
				z += *(lut++);
			}
		}
	}
}

void Fixed::sinCos(Fixed &s, Fixed &c) const
{
	int64_t z = val % TwoPi.val;
	if (z < 0) {
		z += TwoPi.val;
	}

	bool sinNeg = false;
	bool cosNeg = false;
	if (z > Pi.val) {
		z = TwoPi.val - z;
		sinNeg = true;
	}
	if (z > HalfPi.val) {
		z = Pi.val - z;
		cosNeg = true;
	}

	int32_t x = CORDIC_INV_K, y = 0;

	cordicCircularRotation(x, y, int32_t(z));

	s.val = sinNeg ? -y : y;
	c.val = cosNeg ? -x : x;
}

void Fixed::toPolar(const Fixed &x, const Fixed &y, Fixed &r, Fixed &theta)
{
	bool xNeg = x.val < 0;
	bool yNeg = y.val < 0;
	uint64_t a = xNeg ? -x.val : x.val;
	uint64_t b = yNeg ? -y.val : y.val;

	// Scale the operands so that they are small enough for the algorithm to converge
	int rs = 0;
	const unsigned int maxVal = 1 << FIXED_FRAC_BITS;
	while (a >= maxVal || b >= maxVal) {
		rs++;
		a >>= 1;
		b >>= 1;
	}

	int32_t xt = int32_t(a);
	int32_t z = 0;

	cordicCircularVectoring(xt, int32_t(b), z);

	// xt contains the scaled radius, we need to scale it back
	rs = FIXED_FRAC_BITS - rs;
	if (rs < 0)
		r.val = (int64_t(xt) * CORDIC_INV_K) << -rs;
	else
		r.val = (int64_t(xt) * CORDIC_INV_K) >> rs;

	theta.val = z;
	// Determine the quadrant of theta
	if (xNeg && yNeg) {
		theta -= Pi;
	} else if (xNeg) {
		theta = Pi - theta;
	} else if (yNeg) {
		theta = -theta;
	}
}

Fixed Fixed::atan2(const Fixed &y, const Fixed &x)
{
	bool xNeg = x.val < 0;
	bool yNeg = y.val < 0;
	uint64_t a = xNeg ? -x.val : x.val;
	uint64_t b = yNeg ? -y.val : y.val;

	// Scale the operands so that they are small enough for the algorithm to converge
	int rs = 0;
	const unsigned int maxVal = 1 << FIXED_FRAC_BITS;
	while (a >= maxVal || b >= maxVal) {
		rs++;
		a >>= 1;
		b >>= 1;
	}

	int32_t xt = int32_t(a);
	int32_t z = 0;

	cordicCircularVectoring(xt, int32_t(b), z);

	Fixed theta = Fixed(internal(), z);
	// Determine the quadrant of theta
	if (xNeg && yNeg) {
		theta -= Pi;
	} else if (xNeg) {
		theta = Pi - theta;
	} else if (yNeg) {
		theta = -theta;
	}

	return theta;
}

Fixed& Fixed::operator*=(const Fixed &other)
{
	bool selfNeg = val < 0;
	bool otherNeg = other.val < 0;
	bool negate = selfNeg ^ otherNeg;
	uint64_t lhs = selfNeg ? -val : val;
	uint64_t rhs = otherNeg ? -other.val : other.val;
	uint64_t lhsUpper = lhs >> 32;
	uint64_t lhsLower = lhs & 0xffffffff;

	if (lhsUpper != 0) {
		// Multiplying by RHS includes a shift right of FIXED_FRAC_BITS, so
		// we only need to shift by (32 - FIXED_FRAC_BITS) to get upper bits.
		val = (lhsUpper * rhs) << (32 - FIXED_FRAC_BITS);
	} else {
		val = 0;
	}

	if (lhsLower != 0) {
		uint32_t rhsUpper = uint32_t(rhs >> 32);
		uint32_t rhsLower = uint32_t(rhs & 0xffffffff);
		uint64_t lowerUpper = lhsLower * rhsUpper;
		uint64_t lowerLower = lhsLower * rhsLower;
		val += (lowerUpper << (32 - FIXED_FRAC_BITS)) + (lowerLower >> FIXED_FRAC_BITS);
	}

	if (negate) val = -val;

	return *this;
}

Fixed& Fixed::operator/=(const Fixed &other)
{
	assert(other.val != 0);
	if (other.val == 0) {
		*this = (val < 0 ? Ninf : Inf);
		return *this;
	}

	bool selfNeg = val < 0;
	bool otherNeg = other.val < 0;
	bool negate = selfNeg ^ otherNeg;
	uint64_t x = selfNeg ? -val : val;
	uint64_t y = otherNeg ? -other.val : other.val;

	uint64_t temp = y;
	bool xLarger = x > y;
	uint32_t shift = FIXED_FRAC_BITS;

	if (xLarger) {
		// Scale the divisor up to at least half the size of the numerator.
		uint64_t halfX = x >> 1;
		while (temp < halfX) {
			temp <<= 1;
			shift++;
		}
	}
	uint64_t d = uint64_t(1) << shift;
	val = 0;
	if (xLarger) {
		x -= temp;
		val += d;
	}
	while (x != 0 && temp != 0 && shift != 0) {
		uint32_t rs = 0;
		while (rs < shift && (temp > x)) {
			temp >>= 1;
			rs++;
		}
		d >>= rs;
		shift -= rs;
		x -= temp;
		val += d;
	}
	if (negate) val = -val;

	return *this;
}

}} // end namespace
